
#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "basic_sprayer_interfaces/msg/command.hpp"
#include "basic_sprayer_interfaces/msg/sprayer_status.hpp"
#include "basic_sprayer_interfaces/msg/gnss.hpp"
#include "basic_sprayer_interfaces/srv/control_mode.hpp"
#include <functional>
#include "modbus/modbus.h"
#include <stdio.h>
#include <unistd.h>
#include <pthread.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include "bsp_gps.h"

#define DIGITAL2REAL  (1/5000.0)
#define REAL2DIGITAL  5000.0   



using namespace std::chrono_literals;
using std::placeholders::_1;

/* 创建一些全局变量，在整个运行的过程中 */
modbus_t *ctx_sprayer;   // 和大车进行通信
modbus_t *ctx_voltage;   // 读取电池电压
std::shared_ptr<rclcpp::Node> node;
rclcpp::Publisher<basic_sprayer_interfaces::msg::SprayerStatus>::SharedPtr publisher;
rclcpp::Subscription<basic_sprayer_interfaces::msg::Command>::SharedPtr subscription;
rclcpp::Service<basic_sprayer_interfaces::srv::ControlMode>::SharedPtr service;      //服务
rclcpp::TimerBase::SharedPtr timer;    // 定时器

basic_sprayer_interfaces::msg::SprayerStatus status_msg;  //消息的全局变量

int32_t control_mode_set = 0;    //控制模式 0 默认->输入速度和转角  1 ->四个车轮的目标速度 2->四个车轮的目标油门

/* 给GNSS系统预备的数据 */
std::shared_ptr<rclcpp::Node> node_gnss;
rclcpp::Publisher<basic_sprayer_interfaces::msg::Gnss>::SharedPtr publisher_gnss;

basic_sprayer_interfaces::msg::Gnss gnss_msg;

float voltage = 0;          //电池电压

// ctx = modbus_new_rtu("/dev/ttyUSB0", 115200, 'N', 8, 1);
// if (ctx == NULL) {
//     fprintf(stderr, "Unable to create the libmodbus context\n");
//     return -1;
// }

// modbus_set_slave(ctx, YOUR_DEVICE_ID);

// if (modbus_connect(ctx) == -1) {
//     fprintf(stderr, "Connection failed: %s\n", modbus_strerror(errno));
//     modbus_free(ctx);
//     return -1;
// }


/* 接受外界的命令 */
void command_callback(const basic_sprayer_interfaces::msg::Command::SharedPtr msg)
{
    //RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
    /**/
    int16_t array[20];
//    printf("接收到:%f,%f\n",msg->sprayer_speed_set,msg->angle_set);
    //msg->angle_set
    if(ctx_sprayer != NULL){
        switch(control_mode_set){
            default:    //输入速度和转角
            case 0:
                array[0] = msg->sprayer_speed_set*REAL2DIGITAL;
                array[1] = - msg->angle_set*1000.0;
                modbus_write_registers(ctx_sprayer,25,2,(uint16_t *)array);
            break;
            case 1:     //输入四个车轮的目标速度
                array[0] = msg->wheel_speed_set[0]*REAL2DIGITAL;
                array[1] = msg->wheel_speed_set[1]*REAL2DIGITAL;
                array[2] = msg->wheel_speed_set[2]*REAL2DIGITAL;
                array[3] = msg->wheel_speed_set[3]*REAL2DIGITAL;
                modbus_write_registers(ctx_sprayer,27,4,(uint16_t *)array);
            break;
            case 2:     //输入四个车轮的目标油门
                array[0] = msg->throttle_set[0];
                array[1] = msg->throttle_set[1];
                array[2] = msg->throttle_set[2];
                array[3] = msg->throttle_set[3];
                modbus_write_registers(ctx_sprayer,31,4,(uint16_t *)array);
            break;
        }
    }

}

/*定时器，定时读取车的状态 周期运行，获取车的状态 */
void timer_callback()
{
    uint16_t data[10];
    if(ctx_sprayer!=NULL){
        /* 读取信息 */
        if(modbus_read_registers(ctx_sprayer,19,6,data) != -1){
            status_msg.angles[0] = -((int16_t)data[0])/1000.0;
            status_msg.angles[1] = -((int16_t)data[1])/1000.0;
            status_msg.wheel_speed[0] = ((int16_t)data[2])*DIGITAL2REAL;
            status_msg.wheel_speed[1] = ((int16_t)data[3])*DIGITAL2REAL;
            status_msg.wheel_speed[2] = ((int16_t)data[4])*DIGITAL2REAL;
            status_msg.wheel_speed[3] = ((int16_t)data[5])*DIGITAL2REAL;
        }else{
            //printf("modbus sprayer read failed!\r\n");    //打印出来看看
            fprintf(stderr, "modbus sprayer read failed: %s\n", modbus_strerror(errno));
        }
    }
    status_msg.battery_voltage = voltage;
    /* 读取信息 然后发布出去 */
    publisher->publish(status_msg);
}


void control_mode(const std::shared_ptr<basic_sprayer_interfaces::srv::ControlMode::Request> request,
          std::shared_ptr<basic_sprayer_interfaces::srv::ControlMode::Response>    response)
{
    /* 需要在此处直接操作modbus然后返回数据，单线程 不用担心 */
    if(ctx_sprayer == NULL){   //如果链接不成功 直接返回 
        response->is_ok = 0;
    }else{   //这里操作设备，如果成功就返回配置成功信号
        if(request->mode < 3){
            if(modbus_write_register(ctx_sprayer,0,request->mode)!=-1){
                control_mode_set = request->mode;     //设置成功
                response->is_ok = 1;
            }else{
                response->is_ok = 0;
            }
            
        }
    }
}

/* 读取电压的线程 */
pthread_t tid_voltage;
void *thread_voltage(void *p){
    (void)p;
    /* 电压1s钟读取一次 */
    uint16_t value;
    for(;;){
        if(ctx_voltage != NULL){
            if(modbus_read_registers(ctx_voltage,0x0258,1,(uint16_t *)&value) != -1){
                //读取成功
                voltage = value/100.f;   //假设是这个倍率
            }else{
            	fprintf(stderr, "modbus voltage read failed: %s\n", modbus_strerror(errno));
            }
        }
        sleep(1);    //没有外部信号触发时  延时1s
    }
}

int open_port(char* uartname);
int set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop);
/*
    和主控通信，有一个订阅者 和 一个发布者
*/
int stop_flag = 0;
static void sig_int(int signo){   //退出函数
    (void)signo;
    stop_flag = 1;
    printf("get_sig\n");
}

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    if(signal(SIGINT,sig_int) == SIG_ERR){
        perror("signal set error");
    }
    /* 创建了一个节点 */
    node = rclcpp::Node::make_shared("basic_sprayer");      //节点名称

    publisher = node->create_publisher<basic_sprayer_interfaces::msg::SprayerStatus>("sprayer_status",10);
    subscription = node->create_subscription<basic_sprayer_interfaces::msg::Command>("command",2,command_callback);
    service = node->create_service<basic_sprayer_interfaces::srv::ControlMode>("control_mode", &control_mode);
    timer = node->create_wall_timer(50ms,&timer_callback);


    /* 创建modbus设备，如果失败读取，则置错误位 */
    ctx_voltage = modbus_new_rtu("/dev/usb_0", 9600, 'N', 8, 1);
    if (ctx_voltage == NULL) {
        fprintf(stderr, "voltage device create error\n");
        status_msg.errors |= (1l<<63);   //创建电压读取设备时错误
    }else{
        modbus_set_slave(ctx_voltage, 0x1);
        if (modbus_connect(ctx_voltage) == -1) {
            fprintf(stderr, "voltage device connection failed: %s\n", modbus_strerror(errno));
            modbus_free(ctx_voltage);
            ctx_voltage = NULL;
            status_msg.errors |= (1l<<62);   //电压设备连接错误
        }
        modbus_set_response_timeout(ctx_voltage,0,20000);  //200ms
        modbus_set_error_recovery(ctx_voltage,MODBUS_ERROR_RECOVERY_PROTOCOL);
    }

    ctx_sprayer = modbus_new_rtu("/dev/usb_1", 115200, 'N', 8, 1);
    if (ctx_sprayer == NULL) {
        fprintf(stderr, "sprayer device create error\n");
        status_msg.errors |= (1l<<61);          //创建与喷雾机通信设备错误
    }else{
        modbus_set_slave(ctx_sprayer, 0x2);
        if (modbus_connect(ctx_sprayer) == -1) {
            fprintf(stderr, "sprayer device connection failed: %s\n", modbus_strerror(errno));
            modbus_free(ctx_sprayer);
            ctx_sprayer = NULL;
            status_msg.errors |= (1l<<60);      //喷雾机通信设备连接错误
        }
        modbus_set_response_timeout(ctx_sprayer,0,20000);  //200ms
        modbus_set_error_recovery(ctx_sprayer,MODBUS_ERROR_RECOVERY_PROTOCOL);
    }
    /* 如果设备创建成功  则 创建一个线程读取电池电压 */
    if((status_msg.errors & (3l<<62)) == 0){
        if(pthread_create(&tid_voltage,NULL,thread_voltage,NULL) != 0){
            fprintf(stderr, "voltage thread create failed\n");
            status_msg.errors |= (1l<<59);      //创建电压读取进程错误
        }
    }
    /* 创建一个新线程执行这个比较规范的线程 */
    using SingleThreadedExecutor = rclcpp::executors::SingleThreadedExecutor;
    SingleThreadedExecutor executor_command;   //命令接收的
    executor_command.add_node(node);
    std::thread executor_thread(std::bind(&SingleThreadedExecutor::spin, &executor_command));

    /* 接下来是要读取GNSS的数据 */
    bsp_InitGPS();   //初始化结构体中的变量
    node_gnss = rclcpp::Node::make_shared("gnss");      //节点名称
    publisher_gnss = node_gnss->create_publisher<basic_sprayer_interfaces::msg::Gnss>("gnss_position",10);  //创建了一个节点
    int fd=0;
    if((fd=open_port((char*)"/dev/usb_2")) <0 ){
        printf("GNSS open failed\n");
        /* 设置错误位 */
    }else{
        set_opt(fd, 115200, 8, 'N', 1);
    }
    char buffer[1024];                  // 设置保存数据的buff大小
    int read_num;
//    rclcpp::spin(node);                         //看看有没
    while(1){
        if(fd >= 0 ){
            read_num = read(fd, buffer, 1024);   //阻塞读取到空格。
            /*读取到数据之后就可以进行解码，解码之后就可以发送*/
            if(read_num > 0){
                //std::cout<<read_num;
                for(int i =0;i < read_num; i++ ){
                    std::cout<<buffer[i];
                }
                gps_pro((uint8_t *)buffer,read_num);
            }
        }else{
            sleep(1);   //暂时降低一下程序运行占用
            printf("sleep\n");
        }

        // if(g_tGPS.NS == 'N'){
        //     gnss_msg.latitude = g_tGPS.WeiDu_Du + g_tGPS.WeiDu_Fen/(60.0*100000000);
        // }else if(g_tGPS.NS == 'S'){
        //     gnss_msg.latitude = - (g_tGPS.WeiDu_Du + g_tGPS.WeiDu_Fen/(60.0*100000000));
        // }

        // if(g_tGPS.EW == 'E'){
        //     gnss_msg.longitude = g_tGPS.JingDu_Du + g_tGPS.JingDu_Fen/(60.0*100000000);
        // }else if(g_tGPS.EW == 'W'){
        //     gnss_msg.longitude = - (g_tGPS.JingDu_Du + g_tGPS.JingDu_Fen/(60.0*100000000));
        // }
        gnss_msg.latitude = g_tGPS.latitude;
        gnss_msg.longitude = g_tGPS.longitude;
        //gnss_msg.yaw = g_tGPS.Yaw;
        double yaw = g_tGPS.Yaw - 180;
        // if(g_tGPS.Yaw > 180){
        //     yaw = g_tGPS.Yaw - 360;
        // }else if(g_tGPS.Yaw < -180) {
        //     yaw = 360 + g_tGPS.Yaw;
        // }
        gnss_msg.yaw = -yaw;
        publisher_gnss->publish(gnss_msg);
        rclcpp::spin_some(node_gnss);

        if(stop_flag){
            break;
        }
    }
    printf("程序退出\n");    //打印出来看看
    /* 如何结束 接收结束信号 **** */
    close(fd);
//    executor_thread.join();
    rclcpp::shutdown();
    exit(0);
    return 0;
}


/* 打开串口 */
int open_port(char* uartname)
{
//    int fd = open(uartname, O_RDWR|O_NOCTTY|O_NONBLOCK);
    int fd = open(uartname, O_RDWR|O_NOCTTY);
    if (-1 == fd)
    {
        perror("Can't Open Serial Port");
        return(-1);
    }
    /*恢复串口为阻塞状态*/
    // if(fcntl(fd, F_SETFL, 0)<0)
    // {
    //     printf("fcntl failed!\n");
    // }else{
    //     printf("fcntl=%d\n",fcntl(fd, F_SETFL,0));  //直接设置成0就可以，虽然有点不符合规范，但是是可行的。此时是只读的
    // }
    /*测试是否为终端设备*/
    if(isatty(STDIN_FILENO)==0){
        printf("standard input is not a terminal device\n");
    }else{
        printf("isatty success!\n");
    }
    printf("fd-open=%d\n",fd);
    return fd;
}

int set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop)
{
    struct termios newtio,oldtio;
    if  ( tcgetattr( fd,&oldtio)  !=  0) {
        perror("SetupSerial 1");
        return -1;
    }
    bzero( &newtio, sizeof( newtio ) );
    newtio.c_cflag  |=  CLOCAL | CREAD;
//    newtio.c_cflag  |=  CREAD;
    newtio.c_cflag &= ~CSIZE;

    switch( nBits )
    {
    case 7:
        newtio.c_cflag |= CS7;
        break;
    case 8:
        newtio.c_cflag |= CS8;
        break;
    }

    switch( nEvent )
    {
    case 'O':
        newtio.c_cflag |= PARENB;
        newtio.c_cflag |= PARODD;
        newtio.c_iflag |= (INPCK | ISTRIP);
        break;
    case 'E':
        newtio.c_iflag |= (INPCK | ISTRIP);
        newtio.c_cflag |= PARENB;
        newtio.c_cflag &= ~PARODD;
        break;
    case 'N':
        newtio.c_cflag &= ~PARENB;
        break;
    }

    switch( nSpeed )
    {
    case 2400:
        cfsetispeed(&newtio, B2400);
        cfsetospeed(&newtio, B2400);
        break;
    case 4800:
        cfsetispeed(&newtio, B4800);
        cfsetospeed(&newtio, B4800);
        break;
    case 9600:
        cfsetispeed(&newtio, B9600);
        cfsetospeed(&newtio, B9600);
        break;
    case 115200:
        cfsetispeed(&newtio, B115200);
        cfsetospeed(&newtio, B115200);
        break;
    case 460800:
        cfsetispeed(&newtio, B460800);
        cfsetospeed(&newtio, B460800);
        break;
    case 921600:
        printf("B921600\n");
        cfsetispeed(&newtio, B921600);
                cfsetospeed(&newtio, B921600);
        break;
    default:
        cfsetispeed(&newtio, B9600);
        cfsetospeed(&newtio, B9600);
        break;
    }
    if( nStop == 1 )
        newtio.c_cflag &=  ~CSTOPB;
    else if ( nStop == 2 )
        newtio.c_cflag |=  CSTOPB;
    newtio.c_cc[VTIME]  = 0;
    newtio.c_cc[VMIN] = 1;
    newtio.c_lflag |= ICANON;
    tcflush(fd,TCIFLUSH);
    if((tcsetattr(fd,TCSANOW,&newtio))!=0)
    {
        perror("com set error");
        return -1;
    }
  //printf("set done!\n\r");
    return 0;
}





